#ifndef RARDARDATA_H
#define RARDARDATA_H

#include "ros/ros.h"
#include "sensor_msgs/LaserScan.h"
#include <QApplication>
#include <QLabel>
#include <QImage>
#include <QPixmap>
#include <QVBoxLayout>
#include <QWidget>
#include <opencv2/opencv.hpp>
#include "ui.h"


struct point
{
    double x;
    double y;
};

class RadarData{
public:
    RadarData(ros::NodeHandle *nh, Ui_MainWindow *ui);
    void radarCallback(const sensor_msgs::LaserScan::ConstPtr &msg);
    void radarDraw();
    QImage MatToQImage(const cv::Mat &mat);
    cv::Mat radarmap();
private:
    cv::Mat RadarMap_Mat;
    ros::Subscriber radar_sub;
    Ui_MainWindow *ui;
    std::vector<point> points;
    ros::NodeHandle *nh_;
    QImage qtImg; // 用于存储OpenCV图像的QImage对象
    double h_min;
    double v_min;
};

RadarData::RadarData(ros::NodeHandle *nh, Ui_MainWindow *ui):nh_(nh),ui(ui){
    // 订阅雷达数据
    radar_sub = nh->subscribe("/scan", 10, &RadarData::radarCallback, this);
    RadarMap_Mat = radarmap();
}


// 将OpenCV的cv::Mat转换为QImage
QImage RadarData::MatToQImage(const cv::Mat &mat)
{
    // 如果是8位3通道的图像，转换为RGB格式
    if (mat.type() == CV_8UC3)
    {
        QImage image(mat.data, mat.cols, mat.rows, mat.step, QImage::Format_RGB888);
        return image.rgbSwapped(); // OpenCV默认是BGR顺序，需要转换为RGB
    }
    // 如果是8位单通道图像，转换为灰度格式
    else if (mat.type() == CV_8UC1)
    {
        QImage image(mat.data, mat.cols, mat.rows, mat.step, QImage::Format_Grayscale8);
        return image;
    }
    else
    {
        return QImage();
    }
}

cv::Mat RadarData::radarmap(){
    // 使用OpenCV加载或绘制图像
    cv::Mat cvImg(500, 500, CV_8UC3, cv::Scalar(246, 245, 244));  // 创建一个空白图像
    cv::circle(cvImg, cv::Point(250, 250), 1, cv::Scalar(38, 162, 105), -1);
    cv::circle(cvImg, cv::Point(250, 250), 50, cv::Scalar(38, 162, 105), 4);  // 绘制一个半径为50cm的圆
    cv::circle(cvImg, cv::Point(250, 250), 100, cv::Scalar(38, 162, 105), 4);  // 绘制一个半径为100cm的圆
    cv::circle(cvImg, cv::Point(250, 250), 150, cv::Scalar(38, 162, 105), 4);  // 绘制一个半径为150cm的圆
    cv::circle(cvImg, cv::Point(250, 250), 200, cv::Scalar(38, 162, 105), 4);  // 绘制一个半径为200cm的圆

    return cvImg;
}


void RadarData::radarDraw()
{
    cv::Mat cvImg = this->RadarMap_Mat.clone();
    cv::Mat dst;
    int pointsize = 2;
    for (size_t i = 0; i < this->points.size(); ++i)
    {
        // 绘制点
        if (250 - this->points.at(i).x * 100 < 500 - pointsize && 250 - this->points.at(i).y * 100 < 500 - pointsize && 
            250 - this->points.at(i).x * 100 > 0   + pointsize && 250 - this->points.at(i).y * 100 > 0   + pointsize)
        {
            cv::circle(cvImg, 
            cv::Point(250 - this->points.at(i).y * 100, 250 - this->points.at(i).x * 100),
            pointsize, cv::Scalar(0, 0, 255), -1);
        }
    }

    qtImg = MatToQImage(cvImg);
    ui->label->setPixmap(QPixmap::fromImage(qtImg));
//38, 162, 105
// 165, 29, 45
}

// 激光雷达数据的回调函数
void RadarData::radarCallback(const sensor_msgs::LaserScan::ConstPtr& scan)
{
    h_min = 10.0;
    v_min = 10.0;
    // 获取角度范围
    float min_angle = scan->angle_min;
    float max_angle = scan->angle_max;
    float angle_increment = scan->angle_increment;
    float angle;

    // 获取距离数据
    std::vector<float> ranges = scan->ranges;
    if (ranges.size() > this->points.size())
    {
        this->points.resize(ranges.size());
    }

    // 输出每个方位的距离信息
    for (size_t i = 0; i < ranges.size(); ++i)
    {
        // 计算当前扫描的角度 默认第一个射线为0度，默认为x轴正方向
        angle =  i * angle_increment;
        
        // 获取当前角度的距离
        float distance = ranges[i];
        
        // 将距离和角度转换为坐标
        points[i].x = distance * cos(angle);
        points[i].y = distance * sin(angle);

        // 更新最小值
        fabs(distance * cos(angle)) < h_min ? h_min = distance * cos(angle) : h_min = h_min;
        fabs(distance * sin(angle)) < v_min ? v_min = distance * sin(angle) : v_min = v_min;
        for (size_t i = 0; i < 3; i++)
        {
            if(angle < 3.1415926 / 2 * i + angle_increment / 4 && angle > 3.1415926 / 4 * i - angle_increment / 4 ){
                ROS_INFO("ANG:%f x:%f y:%f",angle,distance * cos(angle),distance * sin(angle));
            }
        }
    }
    ROS_INFO("MAX_ANGLE:%f,angle_increment:%f min_angle:%f,max_angle:%f,ranges.size():%d, %f",angle,angle_increment,min_angle,max_angle,ranges.size(),ranges.size()*angle_increment);
}

#endif // RARDARDATA_H